   /** Cholesky Decomposition.
   <P>
   For a symmetric, positive definite matrix A, the Cholesky decomposition
   is an lower triangular matrix L so that A = L*L'.
   <P>
   If the matrix is not symmetric or positive definite, the constructor
   returns a partial decomposition and sets an internal flag that may
   be queried by the isSPD() method.
   */

#include <algorithm>
#include <cmath>
#include "CholeskyDecomposition.hpp"

namespace boost { namespace numeric { namespace ublas {
    
/* ------------------------
   Constructor
 * ------------------------ */

   /** Cholesky algorithm for symmetric and positive definite matrix.
   @param  A   Square, symmetric matrix.
   @return     Structure to access L and isspd flag.
   */

CholeskyDecomposition::CholeskyDecomposition (const Matrix& A) {

     // Initialize.

	  assert( A.size1() <= size_t(std::numeric_limits<int>::max()) );

      n = int(A.size1()); // TODO: cast added by me. 
      L = Matrix(n,n);
      isspd = ((int)A.size2() == n);
      // Main loop.
      for (int j = 0; j < n; j++) {
         matrix_row<Matrix> Lrowj (L, j);
         double d = 0.0;
         for (int k = 0; k < j; k++) {
            matrix_row<Matrix> Lrowk (L, k);
            double s = 0.0;
            for (int i = 0; i < k; i++) {
               s += Lrowk(i)*Lrowj(i);
            }
            Lrowj(k) = s = (A(j,k) - s)/L(k,k);
            d = d + s*s;
            isspd = isspd && (A(k,j) == A(j,k)); 
         }
         d = A(j,j) - d;
         isspd = isspd && (d > 0.0);
         L(j,j) = std::sqrt(std::max(d,0.0));
         for (int k = j+1; k < n; k++) {
            L(j,k) = 0.0;
         }
      }
   }

/* ------------------------
   Temporary, experimental code.
 * ------------------------ *\

   \** Right Triangular Cholesky Decomposition.
   <P>
   For a symmetric, positive definite matrix A, the Right Cholesky
   decomposition is an upper triangular matrix R so that A = R'*R.
   This constructor computes R with the Fortran inspired column oriented
   algorithm used in LINPACK and MATLAB.  In Java, we suspect a row oriented,
   lower triangular decomposition is faster.  We have temporarily included
   this constructor here until timing experiments confirm this suspicion.
   *\

   \** Array for internal storage of right triangular decomposition. **\
   private transient double(,) R;

   \** Cholesky algorithm for symmetric and positive definite matrix.
   @param  A           Square, symmetric matrix.
   @param  rightflag   Actual value ignored.
   @return             Structure to access R and isspd flag.
   *\

   CholeskyDecomposition::CholeskyDecomposition (Matrix Arg, int rightflag) {
      // Initialize.
      double(,) A = Arg.getArray();
      n = Arg.size2();
      R = new double(n,n);
      isspd = (Arg.size2() == n);
      // Main loop.
      for (int j = 0; j < n; j++) {
         double d = 0.0;
         for (int k = 0; k < j; k++) {
            double s = A(k,j);
            for (int i = 0; i < k; i++) {
               s = s - R(i,k)*R(i,j);
            }
            R(k,j) = s = s/R(k,k);
            d = d + s*s;
            isspd = isspd && (A(k,j) == A(j,k)); 
         }
         d = A(j,j) - d;
         isspd = isspd && (d > 0.0);
         R(j,j) = std::sqrt(std::max(d,0.0));
         for (int k = j+1; k < n; k++) {
            R(k,j) = 0.0;
         }
      }
   }

   \** Return upper triangular factor.
   @return     R
   *\

   public Matrix getR () {
      return new Matrix(R,n,n);
   }

\* ------------------------
   End of temporary code.
 * ------------------------ */

/* ------------------------
   Public Methods
 * ------------------------ */

   /** Solve A*X = B
   @param  B   A Matrix with as many rows as A and any number of columns.
   @return     X so that L*L'*X = B
   @exception  bad_size  Matrix row dimensions must agree.
   @exception  singular  Matrix is not symmetric positive definite.
   */

CholeskyDecomposition::Matrix CholeskyDecomposition::solve (const Matrix& B) const {
      BOOST_UBLAS_CHECK((int)B.size1() == n, bad_size("Matrix row dimensions must agree."));
      BOOST_UBLAS_CHECK((int)B.size1() == n, singular("Matrix is not symmetric positive definite."));

      // Copy right hand side.
      Matrix X(B);
	  assert( B.size2() < size_t(std::numeric_limits<int>::max()) );
      int nx = int(B.size2()); // TODO: cast to int added by me

	      // Solve L*Y = B;
	      for (int k = 0; k < n; k++) {
	        for (int j = 0; j < nx; j++) {
	           for (int i = 0; i < k ; i++) {
	               X(k,j) -= X(i,j)*L(k,i);
	           }
	           X(k,j) /= L(k,k);
	        }
	      }
	
	      // Solve L'*X = Y;
	      for (int k = n-1; k >= 0; k--) {
	        for (int j = 0; j < nx; j++) {
	           for (int i = k+1; i < n ; i++) {
	               X(k,j) -= X(i,j)*L(i,k);
	           }
	           X(k,j) /= L(k,k);
	        }
	      }
      
      
      return X;
   }

}}}
